In [1]:
import sys
if not '..' in sys.path:
sys.path.insert(0, '..')
import control
import sympy
import numpy as np
import matplotlib.pyplot as plt
import ulog_tools as ut
from urllib.request import urlopen
import pyulog
%matplotlib inline
%load_ext autoreload
%autoreload 2
In [4]:
def model_to_acc_tf(m):
# open loop, rate output, mix input plant
acc_tf = m['gain'] * control.tf(*control.pade(m['delay'], 1)) # order 1 approx
tf_integrator = control.tf((1), (1, 0))
return acc_tf * tf_integrator
def pid_design(G_rate_ol, d_tc=1.0/125, K0=[0.01, 0.01, 0.01, 5]):
K_rate, G_cl_rate, G_ol_rate_comp = ut.logsysid.pid_design(
G_rate_ol, K0[:3], d_tc)
tf_integrator = control.tf((1), (1, 0))
K, G_cl, G_ol_comp = ut.logsysid.pid_design(
G_cl_rate*tf_integrator, K0[3:], d_tc,
use_I=False, use_D=False)
return np.vstack([K_rate, K])
def pid_tf(use_P=True, use_I=True, use_D=True, d_tc=0.1):
H = []
if use_P:
H += [control.tf(1, 1)]
if use_I:
H += [control.tf((1), (1, 0))]
if use_D:
H += [control.tf((1, 0), (d_tc, 1))]
H = np.array([H]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
return H
In [2]:
log_file = ut.ulog.download_log('http://review.px4.io/download?log=35b27fdb-6a93-427a-b634-72ab45b9609e', '/tmp')
data = ut.sysid.prepare_data(log_file)
res = ut.sysid.attitude_sysid(data)
res
In [3]:
res = ut.sysid.attitude_sysid(data)
res
Out[3]:
In [6]:
fs = ut.ulog.sample_frequency(data)
fs
Out[6]:
$ y[n] = k u[n - d] $
$ y[n] = k z^{-d} u[n] $
$ G = \frac{y[n]}{u[n]} = \frac{k}{z^{d}} $
In [7]:
roll_tf = ut.sysid.delay_gain_model_to_tf(res['roll']['model'])
roll_tf
Out[7]:
In [8]:
pitch_tf = ut.sysid.delay_gain_model_to_tf(res['pitch']['model'])
pitch_tf
Out[8]:
In [9]:
yaw_tf = ut.sysid.delay_gain_model_to_tf(res['yaw']['model'])
yaw_tf
Out[9]:
In [10]:
## Proportional
P_tf = control.tf(1, 1, 1.0/fs)
P_tf
Out[10]:
In [11]:
z, f_s = sympy.symbols('z, f_s')
In [12]:
G_I = z / (f_s*(z-1))
G_I
Out[12]:
In [13]:
I_tf = ut.control.sympy_to_tf(G_I, {'f_s': fs, 'dt': 1.0/fs})
I_tf
Out[13]:
In [14]:
G_D = f_s * (z-1) / z
G_D
Out[14]:
In [15]:
D_tf = ut.control.sympy_to_tf(G_D, {'f_s': fs, 'dt': 1.0/fs})
D_tf
Out[15]:
In [16]:
pid_tf = ut.control.tf_vstack([P_tf, I_tf, D_tf])
pid_tf
Out[16]:
In [17]:
roll_tf_comp = roll_tf*pid_tf
roll_tf_comp
Out[17]:
In [51]:
roll_ss_comp = control.tf2ss(roll_tf_comp);
In [140]:
def attitude_loop_design(m, name):
d_tc = 1.0/10
G_rate_ol = model_to_acc_tf(m)
K0=[0.182, 1.9, 0.01]
K_rate, G_rate_ol, G_rate_comp_cl = ut.logsysid.pid_design(
G_rate_ol, [0.2, 0.2, 0], d_tc)
tf_integrator = control.tf([1], [1, 0])
G_ol = G_rate_comp_cl*tf_integrator
K, G_ol, G_comp_cl = ut.logsysid.pid_design(
G_ol, [1], d_tc,
use_I=False, use_D=False)
K_rate, K
return {
'MC_{:s}RATE_P'.format(name): K_rate[0, 0],
'MC_{:s}RATE_I'.format(name): K_rate[1, 0],
'MC_{:s}RATE_D'.format(name): K_rate[2, 0],
'MC_{:s}_P'.format(name): K[0, 0],
}
In [141]:
log_file = ut.ulog.download_log('http://review.px4.io/download?log=35b27fdb-6a93-427a-b634-72ab45b9609e', '/tmp')
data = ut.sysid.prepare_data(log_file)
res = ut.sysid.attitude_sysid(data)
res
Out[141]:
In [142]:
attitude_loop_design(res['roll']['model'], 'ROLL')
Out[142]:
In [143]:
attitude_loop_design(res['pitch']['model'], 'PITCH')
Out[143]:
In [144]:
attitude_loop_design(res['yaw']['model'], 'YAW')
Out[144]:
In [ ]: